/**
* @file        SensorDataCache
* @brief       传感器信号的 cache
* @version     1.0.0
* @author      Changlin.Jing 
* @date        2022/10/19 19:57
* @copyright   Copyright  2022 Langge Software Co., Ltd. All rights reserved.
*/
#ifndef LANGGEBASE_SENSORDATACACHE_HPP
#define LANGGEBASE_SENSORDATACACHE_HPP
#include "ut_symbols.h"
#include "loc_types.h"
#include "common/ut_vector_buff.h"

BEGIN_SPACE_LANGE_VDR
class LocationQualityEstimatorResult{
public:
    int32_t    mFixedSatelliteNum = 0; //定位使用的卫星数量
    double     mAvgSNR = 0.0;          //信噪比
    uint64_t   mGsvTimestamp = 0L;     //时间戳
    int32_t    mTotalSatelliteNum = 0; //可见卫星数量
    double     mGsvScore = 0.0;
};
class SensorDataCache{
private:
    SensorDataCache();
    static SensorDataCache * mInstance;
public:
    static SensorDataCache * GetInstance();

    static void ReleaseInstance();
    virtual ~SensorDataCache();
    void reset();

    void storeData(const std::shared_ptr<I_LOC_SIGNAL>& posSignal);
    std::vector<float> getAHRS() const;

private:
    void initPos(const std::shared_ptr<LOC_POS_SIGNAL>& sigPos);

    void addRawSignal(const std::shared_ptr<LOC_POS_SIGNAL>& sigPos);

    void addAccSignal(const std::shared_ptr<LOC_ACC_SIGNAL>& sigAcc);

    void addGyrSignal(const std::shared_ptr<LOC_GYRO_SIGNAL>& sigGyro);

    void addMagSignal(const std::shared_ptr<LOC_MAGN_SIGNAL>& sigMag);
    void addCarSignal(const std::shared_ptr<LOC_CAR_SIGNAL> &sigCar);

    SensorDataCache & operator=(const SensorDataCache &other);

    void CheckWorkStateBySensor();


public:
    std::uint64_t mLocalTickTime{};
    bool mSensorInit{},mPosInit{};
    std::uint64_t mAccLastTimeStamp{};
    std::uint64_t mGyrLastTimeStamp{};
    std::uint64_t mMagLastTimeStamp{};
    std::uint64_t mRawLastTimeStamp{};
    std::int64_t mCarLastTimeStamp{};
    // 上一个原始信号用于过滤计算
    std::shared_ptr<LOC_POS_SIGNAL> mLastPosSignal = nullptr;
    // 最新的过滤后的信号，用于对外的输出
    std::shared_ptr<LOC_POS_SIGNAL> mLastFilteredPosSignal = nullptr;
    std::shared_ptr<LOC_ACC_SIGNAL> mLastAccSignal = nullptr;
    std::shared_ptr<LOC_GYRO_SIGNAL> mLastGyroSignal = nullptr;
    std::shared_ptr<LOC_CAR_SIGNAL> mLastCarSignal = nullptr;

    // 传感器信号 1 秒钟 大概是 3 秒 保留了 3 秒的滑窗
    const static int mSizeWindow = 75;
    constexpr static float gravity = 9.81f;
    const static int mCarSizeWindow = 20;

    int mIndexAcc{},mIndexGyr{},mIndexMag{},mIndexCar;
    bool mIsAccFull{},mIsGyrFull{},mIsMagFull{},mIsCarFull{};

    std::shared_ptr<LOC_ROTATION_SIGNAL> mRotation = std::make_shared<LOC_ROTATION_SIGNAL>();

    std::shared_ptr<LocationQualityEstimatorResult> mLocationQualityEstimatorResult = std::make_shared<LocationQualityEstimatorResult>();

    std::shared_ptr<LOC_SATELLITE_SIGNAL> mPtrLastSatelliteSignal;

    std::vector<float> mRotationMatrix = {1,0,0,0,1,0,0,0,1};

    std::vector<float> mLastAcc;
    std::vector<float> mLastGyr;
    std::vector<float> mLastMag;

    std::vector<float> mLastOriginAcc;
    std::vector<float> mLastOriginGyr;
    std::vector<float> mLastOriginMag;

    std::vector<float> mAccVerticalData;
    std::vector<float> mAccYOriginData;
    std::vector<float> mAccHorizonData;
    std::vector<float> mAccTotalData;
    std::vector<float> mAccVerticalDataDelta;
    std::vector<float> mAccHorizonDataDelta;
    std::vector<float> mAccTotalDataDelta;

    std::vector<float> mAccBearingData;
    std::vector<float> mGyrVerticalData;
    std::vector<float> mGyrHorizonData;
    std::vector<float> mGyrTotalData;
    std::vector<float> mGyrVerticalDataDelta;
    std::vector<float> mGyrHorizonDataDelta;
    std::vector<float> mGyrTotalDataDelta;

    //CAR 缓存 也保存 15s 75个 设定 1Hz ~ 5Hz
    std::vector<float> mCarData;
    //最新的车速（m/s）
    float mLastCarSPeed = -1.f;

    float mLastTurnRate{};
    float tureRateK = 0.8f;

    std::vector<float> mMagVerticalData;
    std::vector<float> mMagHorizonData;
    std::vector<float> mMagTotalData;

    std::vector<float> mMagVerticalDataDelta;
    std::vector<float> mMagHorizonDataDelta;
    std::vector<float> mMagTotalDataDelta;

    //*GPS 缓存*/
    VectorBuffer<std::uint64_t,double>* mRawCache{};
    std::vector<double> mLastRaw;

    bool isPlatformAuto = false;
    static const std::uint32_t mCacheDim = 11;
    // GPS 保留了 15 个
    static const std::uint32_t sCacheSize = 15;

    static const std::uint64_t mGpsTimeout = 5000;
    static const std::uint64_t mSensorTimeout = 1000;

    std::vector<double> mLlo;
    std::vector<double> mFlatXy;
    std::vector<double> mAccGpsBearingDeltaData;
    bool hasSensorWorkStateCheck = false;



};
END_SPACE_LANGE_VDR
#endif //LANGGEBASE_SENSORDATACACHE_HPP
